﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using Microsoft.Kinect;
using KSASupport.Calibration;
using System.Threading.Tasks;
using System.Windows;
using KSASupport.Warping;


namespace KSASupport
{
    /// <summary>
    /// Dpeth managers are the main lower level class of KSA.  For each kinect, a depth manager is created to handle its depth frame recieved events.  For
    /// each frame, the depth manager handles getting depth events.  It is the glue that combines the calibration with the blob detector.  It also holds handles to the
    /// sensors themselves.  
    /// </summary>
    public partial class DepthManager
    {

        private short[] rawDepthValues;
        /// <summary>
        /// The raw depth data from the Kinect Sensor
        /// </summary>
        public short[] RawDepthValues
        {
            get { return rawDepthValues; }
        }

        /// <summary>
        /// Tracks if the depthmanager/Kinect has been calibrated yet. If you need to calibrate..set this to false first (this is as a failsafe to
        /// prevent ruining existing calibrations!)
        /// </summary>
        public bool IsCalibrated
        {
            get { return depthCalibrationManager.IsCalibrated;  }
            set
            {
                depthCalibrationManager.IsCalibrated = value;
            }
        }

        private KinectSensor kinectSensor;
        /// <summary>
        /// Handle to the Kinect Sensor used for this Depth Manager
        /// </summary>
        public KinectSensor KinectSensor
        {
            get { return kinectSensor; }
            set { kinectSensor = value; }
        }

        private int[] calibratedDepthValues;
        /// <summary>
        /// This tracks the distance each detected item is off the screen
        /// </summary>
        public int[] CalibratedDepthValues
        {
            get { return calibratedDepthValues; }
        }

        private DepthManagerSettings depthManagerSettings;
        /// <summary>
        /// Stores settings such as calibration values, depth settings, and touch point mappings
        /// </summary>
        public DepthManagerSettings DepthManagerSettings
        {
            get { return depthManagerSettings; }
            set { depthManagerSettings = value; }
        }

        private int[] thresholdImage;
        /// <summary>
        /// A thresholded version of the calibrated Image..should be much cleaner
        /// </summary>
        public int[] ThresholdImage
        {
            get { return thresholdImage; }
            set { thresholdImage = value; }
        }

        private int[] hoveringThresholdImage;
        public int[] HoveringThresholdImage
        {
            get { return hoveringThresholdImage; }
            set { hoveringThresholdImage = value; }
        }

        List<DepthBlob> depthBlobs;

        public List<DepthBlob> DepthBlobs
        {
            get { return depthBlobs; }
            set { depthBlobs = value; }
        }

        private List<DepthBlob> hoveringBlobs;
        public List<DepthBlob> HoveringBlobs
        {
            get { return hoveringBlobs; }
            set { hoveringBlobs = value; }
        }

        private BlobDetector blobDetector;

        public BlobDetector BlobDetector
        {
            get { return blobDetector; }
            set { blobDetector = value; }
        }

        private BlobDetector hoveringBlobDetector;

        public BlobDetector HoveringBlobDetector
        {
            get { return hoveringBlobDetector; }
            set { hoveringBlobDetector = value; }
        }

        /// <summary>
        /// Warper is the class used to do a 4 point mapping between the Kinect
        /// image and the screen coordinates. Each Kinect and screen combo will
        /// have different mappings, so each DepthManager needs to be aware of it's mapping
        /// </summary>
        private Warper warper;        
        public Warper Warper
        {
            get { return warper; }
            set { warper = value; }
        }



        private bool enabled;
        /// <summary>
        /// Use this to turn off touch / processing events, or to enable touch/processing
        /// </summary>
        public bool Enabled
        {
            get { return enabled; }
            set { enabled = value; }
        }

        // Watches for how many frames we've not reported due to not finding any pixels
        // if this goes over a certain amount, we send up the empty frame to be processed
        // This will cause latency in the situation where all contacts are removed from the screen
        // but will provide us with protection if the Kinect doesn't report any data on a frame
        private int SkippedFrames = 0;

        /// <summary>
        /// This gets built by the DCM to provide a visual of the calibration process
        /// </summary>
        public byte[] CalibrationImage
        {
            get { return depthCalibrationManager.CalibrationImage; }
        }

        #region Depth Attribute Fields
        private int depthImageWidth;

        public int DepthImageWidth
        {
            get { return depthImageWidth; }
            set { depthImageWidth = value; }
        }

        private int depthImageHeight;

        public int DepthImageHeight
        {
            get { return depthImageHeight; }
            set { depthImageHeight = value; }
        }
        
        #endregion


        private KinectManager kinectManager; // manages the Kinect instance
        private DepthCalibrationManager depthCalibrationManager;
        
        /// <summary>
        /// Exposes the low level calibration manager which stores calibration information. Should mainly be used
        /// by apps which are tweaking the calibration data.
        /// </summary>
        public DepthCalibrationManager DepthCalibrationManager
        {
            get { return depthCalibrationManager; }
        }

        /// <summary>
        /// Instantiates the Depthmanager. The 'kinect to use' is the index of the currently connected Kinect Sensor
        /// </summary>
        /// <param name="KinectToUse"></param>
        public DepthManager(int KinectToUse)
        {
            kinectManager = new KinectManager();           
            
            kinectManager.Initialize(KinectToUse, DepthImageFormat.Resolution320x240Fps30, DepthRange.Default);
            kinectSensor = kinectManager.KinectSensor;
            
            depthImageHeight = kinectManager.KinectSensor.DepthStream.FrameHeight;
            depthImageWidth = kinectManager.KinectSensor.DepthStream.FrameWidth;
            string SensorID = kinectManager.KinectSensor.UniqueKinectId;

            string SensorName = SensorID.Substring(SensorID.LastIndexOf('\\'));
            depthManagerSettings = new KSASupport.DepthManagerSettings(SensorName,depthImageWidth,depthImageHeight);

            depthCalibrationManager = new DepthCalibrationManager(depthManagerSettings);                    

            depthBlobs = new List<DepthBlob>();
            HoveringBlobs = new List<DepthBlob>();
                        
            blobDetector = new BlobDetector(depthImageWidth, depthImageHeight,depthManagerSettings.ScreenBoundaryRect);
            blobDetector.MinimumBlobSPixelCount = 15;
            // TODO: Parametize these in the Depthmanager Settings
            blobDetector.MinimumBlobPixelWidth  = 4;
            blobDetector.MinimumBlobPixelHeight = 4;

            hoveringBlobDetector = new BlobDetector(depthImageWidth, depthImageHeight, depthManagerSettings.ScreenBoundaryRect);
            hoveringBlobDetector.MinimumBlobSPixelCount = 45;

            warper = new Warper();
        }
        // Default constructor for a single Kinect
        public DepthManager() : this(0)
        {
            
        }

        /// <summary>
        /// Starts Kinect and pumps for image info
        /// </summary>
        public void Initialize()
        {
            if (kinectManager.GetKinectStatus() == Microsoft.Kinect.KinectStatus.Connected)
            {
                kinectManager.KinectSensor.DepthFrameReady += new EventHandler<Microsoft.Kinect.DepthImageFrameReadyEventArgs>(KinectSensor_DepthFrameReady);
            }
            rawDepthValues = new short[kinectManager.KinectSensor.DepthStream.FramePixelDataLength];
            depthCalibrationManager = new Calibration.DepthCalibrationManager(depthManagerSettings);

            // Attempt to read calibration data
            // TODO: Should check return value...what should we do if this fails?
            ReadCalibration();


            // BUGBUG: Handle 'kinect not ready' exception here.
            kinectManager.KinectSensor.Start();
            // dynamically default to 'near' mode
            try
            {
                kinectManager.KinectSensor.DepthStream.Range = DepthRange.Near;
            }
            catch (Exception)
            {
                kinectManager.KinectSensor.DepthStream.Range = DepthRange.Default;
            }
        }
        #region Calibration Methods

        /// <summary>
        /// Starts Kinect and Calibrates depth image
        /// </summary>
        public void Calibrate()
        {
            Initialize();
            kinectManager.KinectSensor.DepthFrameReady -= new EventHandler<Microsoft.Kinect.DepthImageFrameReadyEventArgs>(KinectSensor_DepthFrameReady);
            kinectManager.KinectSensor.DepthFrameReady += new EventHandler<Microsoft.Kinect.DepthImageFrameReadyEventArgs>(KinectSensor_CalibrateDepthFrameReady);
            depthCalibrationManager.StartCalibration();            
        }

        public void StopCalibration()
        {
            kinectManager.KinectSensor.DepthFrameReady -= new EventHandler<DepthImageFrameReadyEventArgs>(KinectSensor_CalibrateDepthFrameReady);
        }

        void KinectSensor_CalibrateDepthFrameReady(object sender, Microsoft.Kinect.DepthImageFrameReadyEventArgs e)
        {
            if (depthCalibrationManager.IsCalibrating)
            {
                using (DepthImageFrame imageFrame = e.OpenDepthImageFrame())
                {
                    if (imageFrame != null)
                    {
                        // There are various methods to calibrate:
                        // Plane - Used for flat surfaces such as a wall or a television screen. Requires
                        //      4 points, and then interpolates depth based on those 4 points.
                        // PerPixel - Used for any uneven surface, rounded surfaces, etc. 
                        switch (depthManagerSettings.CalibrationMethod)
                        {
                                
                            case CalibrationMethods.Plane:
                                {
                                    // Note: The next method was an experimental calibration technique
                                    // for a 'plane' using 4 points to setup the calibration. It's...not much
                                    // better from the per pixel one I started with. But since I had it at
                                    // least working, I left it in in case I decide to iterate on it.
                                    depthCalibrationManager.ProcessImageFrameForCalibration(imageFrame,
                                       depthManagerSettings.ScreenBoundaryRect.TopRight,
                                        depthManagerSettings.ScreenBoundaryRect.BottomRight,
                                       depthManagerSettings.ScreenBoundaryRect.BottomLeft,
                                        depthManagerSettings.ScreenBoundaryRect.TopLeft);

                                    break;
                                }
                            case CalibrationMethods.PerPixel:
                                {
                                    depthCalibrationManager.ProcessImageFrameForCalibration(imageFrame);
                                    break;
                                }
                        }

                        
                        
                    }
                }
            }
            else
            {
                StopCalibration();
            }
        }
#endregion

        #region Kinect Control
        // Note: Not sure if we need this just yet. May come in handy for multi-kinect scenarios.
        /// <summary>
        /// Stops the Kinect Sensor
        /// </summary>
        public void Stop()
        {
            kinectManager.KinectSensor.Stop();
        }
        /// <summary>
        /// Starts the sensor, thus firing the DepthFrameReady loop
        /// </summary>
        public void Start()
        {
            kinectManager.KinectSensor.Start();
        }
        #endregion

        /// <summary>
        /// Main 'loop', fired every time the depth frame is ready from the Kinect
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        void KinectSensor_DepthFrameReady(object sender, Microsoft.Kinect.DepthImageFrameReadyEventArgs e)
        {
            using (DepthImageFrame imageFrame = e.OpenDepthImageFrame())
            {
                // race condition means we may call this while trying to calibrate...
                if (depthCalibrationManager.IsCalibrating)
                    return;

                if (imageFrame != null)
                {
                    // If we're not calibrated, show the 'raw image'. Else generate an image based on the calibration
                    // and whatevers over the screen                   

                    imageFrame.CopyPixelDataTo(rawDepthValues);

                    if (depthCalibrationManager.IsCalibrated)
                    {
                        if (thresholdImage != null)
                        {
                            thresholdImage = new int[depthImageWidth * depthImageHeight];
                        }
                        if (hoveringThresholdImage == null)
                        {
                            // Note: In the future we may want to expand the size of this to cover edges
                            hoveringThresholdImage = new int[depthImageWidth * depthImageHeight];
                        }

                        GenerateDepthImage(imageFrame);

                        int thresholdPixelCount;
                        thresholdImage = blobDetector.GetThresholdImage(calibratedDepthValues,
                            (int)depthManagerSettings.LowestDepthFromSurface,
                            (int)depthManagerSettings.HighestDepthFromSurface,out thresholdPixelCount);

                        // Reduce flickering via "Weighted average" method. Disabled currently due to blur it creates
                        // I believe there is a better way to do this by increasing the strictness
                        //thresholdImage = blobDetector.CreateAverageDepthArray(thresholdImage);
                        int hoveringPixelCount;
                        hoveringThresholdImage = blobDetector.GetThresholdImage(calibratedDepthValues,
                            (int)depthManagerSettings.HighestDepthFromSurface, 100000,out hoveringPixelCount);

                        if ((Enabled))
                        {
                            // NOTE: This will break hover scenarios!
                            if ((thresholdPixelCount > depthManagerSettings.MinimumBlobPixelCount) || (SkippedFrames > 1))
                            {
                                blobDetector.MinimumBlobSPixelCount = depthManagerSettings.MinimumBlobPixelCount;
                                depthBlobs = BlobDetector.GetBlobsFromDepthImage(thresholdImage, this.kinectManager.KinectSensor.UniqueKinectId, "TouchLayer", false);
                                hoveringBlobs = BlobDetector.GetBlobsFromDepthImage(hoveringThresholdImage, this.kinectManager.KinectSensor.UniqueKinectId, "HoverLayer", true);
                                OnFrameReceived(new FrameReceivedEventArgs() { CurrentBlobs = depthBlobs, HoveringBlobs = hoveringBlobs });
                                SkippedFrames = 6;
                            }
                            
                            
                        }
                        if (thresholdPixelCount < depthManagerSettings.MinimumBlobPixelCount)
                        {
                            // TODO: Disabled!
                            //SkippedFrames++;
                        }
                        
                    }                     
                }
            }
        }

        /// <summary>
        /// Gets a raw depth frame from the Kinect
        /// </summary>
        /// <param name="depthImageFrame"></param>
        public void GenerateDepthImage(DepthImageFrame depthImageFrame)
        {

            Parallel.For(0,depthImageFrame.PixelDataLength, index =>
               {
                   int len = depthCalibrationManager.CalibrationGrid.Length;
                   if (depthCalibrationManager.CalibrationGrid[index] == null)
                   {
                       calibratedDepthValues[index] = 0;
                   }
                   else
                   {
                       calibratedDepthValues[index] =
                          depthCalibrationManager.CalibrationGrid[index].GetDistanceFromCalibratedPixel(rawDepthValues[index] >> DepthImageFrame.PlayerIndexBitmaskWidth);
                   }
               });
        }

        /// <summary>
        /// Call this to force reading of the calibration system.
        /// </summary>
        public void ReadCalibration()
        {
            rawDepthValues = new short[kinectManager.KinectSensor.DepthStream.FramePixelDataLength];
            calibratedDepthValues = new int[kinectManager.KinectSensor.DepthStream.FramePixelDataLength];
            //depthCalibrationManager.ReadCalibration();
            depthManagerSettings.ReadSettings();
            ReadPointMapping();
        }

        /// <summary>
        /// sets up warper class with calibrated points
        /// </summary>
        private void ReadPointMapping()
        {
            // we store everything as a WPF point for ease, but we calculate everything against floats
            // this requires we either cast everything..since its only 30 or so casts, and its at load time
            // we let this slide. Should investigate if we should switch to storing the points as seperate floats
            // to improve load times, though we'd probably not notice it.
            warper.SetSource(
                (float)depthManagerSettings.CalibrationTopLeftSource1.X,
                (float)depthManagerSettings.CalibrationTopLeftSource1.Y,
                (float)depthManagerSettings.CalibrationBottomLeftSource1.X,
                (float)depthManagerSettings.CalibrationBottomLeftSource1.Y,
                (float)depthManagerSettings.CalibrationBottomRightSource1.X,
                (float)depthManagerSettings.CalibrationBottomRightSource1.Y,
                (float)depthManagerSettings.CalibrationTopRightSource1.X,
                (float)depthManagerSettings.CalibrationTopRightSource1.Y);

            warper.SetDestination(
                (float)depthManagerSettings.CalibrationTopLeftDestination1.X,
                (float)depthManagerSettings.CalibrationTopLeftDestination1.Y,
                (float)depthManagerSettings.CalibrationBottomLeftDestination1.X,
                (float)depthManagerSettings.CalibrationBottomLeftDestination1.Y,
                (float)depthManagerSettings.CalibrationBottomRightDestination1.X,
                (float)depthManagerSettings.CalibrationBottomRightDestination1.Y,
                (float)depthManagerSettings.CalibrationTopRightDestination1.X,
                (float)depthManagerSettings.CalibrationTopRightDestination1.Y);             
        }

        /// <summary>
        ///  cleanup
        /// </summary>
        public void Uninitialize()
        {
            rawDepthValues = null;
            kinectManager.KinectSensor.DepthFrameReady -= new EventHandler<Microsoft.Kinect.DepthImageFrameReadyEventArgs>(KinectSensor_DepthFrameReady);
            kinectManager.KinectSensor.DepthFrameReady -= new EventHandler<DepthImageFrameReadyEventArgs>(KinectSensor_CalibrateDepthFrameReady);
            kinectManager.Uninitialize();
            kinectManager.KinectSensor.Dispose();
        }

    }
}
